#include "PFC.h"

FSM_Status_t FSM_Status;
Fault_Flags_t PFC_Faults;
Working_Mode_t Working_Mode;

Delay_Buffer_t Grid_Vol_Buffer,Grid_Cur_Buffer;
SPLL_1PH_SOGI Grid_SPLL;
POWER_MEAS_SINE_ANALYZER Grid_Analyzer;

Pid_Controller_t GI_Id_Ctrl,GI_Iq_Ctrl,GV_VBus_Ctrl,GV_VGrid_Ctrl;

float VBus,Angle_Radians,Internal_Sine,Internal_Cosine,Last_VGrid;
alphabeta_t VGrid_ab,IGrid_ab,Duty_ab;
qd_t VGrid_qd,IGrid_qd,Duty_qd;

float IGrid_q_ref,IGrid_d_ref,VBus_ref,VGrid_ref,I_ref;
volatile float duty; 
uint16_t ADC_RawData[4];

extern comkey_t key0,key1,key2;

int key0_state_last,key0_state,key1_state_last,key1_state,key2_state_last,key2_state;

void Buffer_Init(Delay_Buffer_t* p)
{
	int i;
    for(i=0;i<BUFFER_SIZE;i++)
        p->data[i] = 0;
    p->front = 0;
	p->rear = 0;
}

void PFC_Init()
{
    FSM_Status = status_WaitForGrid;
    Working_Mode = mode_CurrentSource;

    I_ref = 1.5;
    IGrid_q_ref = (I_ref*IRMS_COEFF+IRMS_BIAS)*CURRENT_COMPENSATE_COS;
    IGrid_d_ref = (I_ref*IRMS_COEFF+IRMS_BIAS)*CURRENT_COMPENSATE_SIN;

    Duty_qd.q = -PFC_DEFAULT_DUTY;
    Duty_qd.d = 0;

    VGrid_ref = 23.9;

    Buffer_Init(&Grid_Vol_Buffer);
    Buffer_Init(&Grid_Cur_Buffer);

    SPLL_1PH_SOGI_reset(&Grid_SPLL);
    SPLL_1PH_SOGI_config(&Grid_SPLL, GRID_FREQ_HZ, PFC_ISR_FREQ_HZ, 222.2862f, -222.034f);

    POWER_MEAS_SINE_ANALYZER_reset(&Grid_Analyzer);
    POWER_MEAS_SINE_ANALYZER_config(&Grid_Analyzer, PFC_ISR_FREQ_HZ, 0.0f, 65.0f, 45.0f);

    Pid_Init(&GI_Id_Ctrl, PFC_GI_KP, PFC_GI_KI, 0, 0, 1/PFC_ISR_FREQ_HZ, PFC_GI_LIMIT);
    Pid_Init(&GI_Iq_Ctrl, PFC_GI_KP, PFC_GI_KI, 0, 0, 1/PFC_ISR_FREQ_HZ, PFC_GI_LIMIT);
    Pid_Init(&GV_VBus_Ctrl, PFC_GV_KP, PFC_GV_KI, 0, 0, 1/100.0, PFC_GV_LIMIT);
    Pid_Init(&GV_VGrid_Ctrl,PFC_GV_KP, PFC_GV_KI, 0, 0, 1/200.0, PFC_GV_LIMIT);

    HAL_ADC_Start_DMA(&hadc2,(uint32_t*)ADC_RawData,4);
    HAL_HRTIM_WaveformCounterStart(&hhrtim1,HRTIM_TIMERID_TIMER_A|HRTIM_TIMERID_TIMER_B|HRTIM_TIMERID_MASTER);
    
    PFC_Peripheral_Setup();

    Gate_Disable();
    Relay_Open();

    Menu_Init();

    HAL_TIM_Base_Start_IT(&htim16);    
}

void PFC_Peripheral_Setup()
{
    HAL_Delay(50);//Wait for peripherals to power up
    INA228_CONFIGTypeDef INA228_Config={0x01,0x01,0xFF,0x01,0x00,4.096};
    INA228_ADCTypeDef INA228_ADCConfig={0xF, 0x07, 0x07, 0x07, 0x01};
    INA228_Init(&INA228_Config);
    INA228_ADC_Config(&INA228_ADCConfig);

    OLED_Init();
    OLED_Display_On();
}

void PFC_While()
{
//    OLED_ShowNum(2,4,sb, 2, 16);

    key0_state_last = key0_state;
    key1_state_last = key1_state;
    key2_state_last = key2_state;
    key0_state = HAL_GPIO_ReadPin(B1_GPIO_Port,B1_Pin);
    key1_state = HAL_GPIO_ReadPin(KEY_1_GPIO_Port,KEY_1_Pin);
    key2_state = HAL_GPIO_ReadPin(KEY_2_GPIO_Port,KEY_2_Pin);

    if (!key0_state && key0_state_last)
    {
        Menu_Callback(0);
    }    
    if (!key1_state && key1_state_last)
    {
        Menu_Callback(1);
    }    
    if (!key2_state && key2_state_last)
    {
        Menu_Callback(2);
    }

    float data[10];
    uint8_t tail[4] = {0x00, 0x00, 0x80, 0x7f};

    data[0] = Grid_Analyzer.vRms;
    data[1] = Grid_SPLL.fo;
    data[2] = (float)FSM_Status;
    data[3] = VGrid_ab.alpha;

    //OLED_Clear();
    //OLED_ShowString(2,4,"sb",16);

#ifdef INVERTER_1
    HAL_UART_Transmit(&huart1,(uint8_t*)data,sizeof(float)*3,0xFF);
    HAL_UART_Transmit(&huart1,(uint8_t*)tail,4,0xFF);
#endif

#ifdef INVERTER_2
    HAL_UART_Transmit(&hlpuart1,(uint8_t*)data,sizeof(float)*3,0xFF);
    HAL_UART_Transmit(&hlpuart1,(uint8_t*)tail,4,0xFF);
#endif    
}

